import cv2

class Imx219Camera:
    def __init__(self, device, name):
        self.device = device
        self.name = name
        self.cap = None
        
        # 支持的像素格式映射表
        self.fourcc_map = {
            'MJPG': cv2.VideoWriter_fourcc(*'MJPG'),
            'YUYV': cv2.VideoWriter_fourcc(*'YUYV'),
            'UYVY': cv2.VideoWriter_fourcc(*'UYVY'),
            'YVYU': cv2.VideoWriter_fourcc(*'YVYU'),
            'YUY2': cv2.VideoWriter_fourcc(*'YUY2'),
            'H264': cv2.VideoWriter_fourcc(*'H264'),
            'NV12': cv2.VideoWriter_fourcc(*'NV12'),
            'NV21': cv2.VideoWriter_fourcc(*'NV21'),
            'YV12': cv2.VideoWriter_fourcc(*'YV12'),
            'RGB3': cv2.VideoWriter_fourcc(*'RGB3'),
            'BGR3': cv2.VideoWriter_fourcc(*'BGR3'),
            'GREY': cv2.VideoWriter_fourcc(*'GREY')
        }

    def open_gstreamer(self, width=1920, height=1080, fps=30):
        pipeline = (
            f"v4l2src device={self.device} ! "
            f"video/x-raw,format=NV12,width={width},height={height},framerate={fps}/1 ! "
            "videoconvert ! appsink"
        )

        self.cap = cv2.VideoCapture(pipeline, cv2.CAP_GSTREAMER)
        return self._check_opened()

    def open_v4l2(self, width=1920, height=1080, fps=30, pixel_format='NV12'):
        self.cap = cv2.VideoCapture(self.device, cv2.CAP_V4L2)
        if not self._check_opened():
            return False

        # 设置像素格式
        if not self._set_pixel_format(pixel_format):
            return False

        # 设置摄像头参数
        self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, width)
        self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, height)
        self.cap.set(cv2.CAP_PROP_FPS, fps)
        
        return True

    def _set_pixel_format(self, pixel_format):
        if pixel_format in self.fourcc_map:
            success = self.cap.set(cv2.CAP_PROP_FOURCC, self.fourcc_map[pixel_format])
            if not success:
                print(f"[WARNING] 设置格式 {pixel_format} 失败，使用默认格式")
        else:
            print(f"[WARNING] 不支持的格式 {pixel_format}，使用默认格式")
        return True

    def _check_opened(self):
        if not self.cap or not self.cap.isOpened():
            print(f"[ERROR] 无法打开摄像头 {self.name} ({self.device})")
            return False
        print(f"[INFO] 打开摄像头 {self.name} ({self.device}) 成功")
        return True

    def read(self):
        if not self.cap or not self.cap.isOpened():
            print(f"[ERROR] 摄像头 {self.name} 未打开或已关闭")
            return None

        ret, frame = self.cap.read()
        if not ret:
            print(f"[ERROR] 摄像头 {self.name} 读取图像失败")
            return None
            
        return frame

    def is_opened(self):
        return self.cap and self.cap.isOpened()

    def release(self):
        if self.cap:
            self.cap.release()
            self.cap = None
            print(f"[INFO] 摄像头 {self.name} 资源已释放")
